landmark_base.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_state_blocks.h"
#include "core/common/time_stamp.h"
#include "core/landmark/factory_landmark.h"
 
// yaml
#include "yaml-schema-cpp/yaml_server.hpp"
 
namespace wolf
{
/*
 * Macro for defining Autoconf landmark creator.
 *
 * Place a call to this macro inside your class declaration (in the landmark_class.h file),
 * preferably just after the constructors. Inside 'public' keyword.
 *
 * In order to use this macro, the derived landmark class, LandmarkClass,
 * must have two constructors available with the API:
 *
 *   LandmarkClass(const YAML::Node& _node);
 *
 * Also, there should be the schema file 'LandmarkClass.schema' containing the specifications
 * of the user input yaml file.
 *
 * NOTE: We use `new` and not `make_shared` since the constructor may (should) be private/protected
 */
#define WOLF_LANDMARK_CREATE(LandmarkClass)                                                                           \
    static LandmarkBasePtr create(const YAML::Node& _node, const std::vector<std::string>& _folders_schema = {})      \
    {                                                                                                                 \
        if (not _folders_schema.empty())                                                                              \
        {                                                                                                             \
            auto server = yaml_schema_cpp::YamlServer(_folders_schema);                                               \
            server.setYaml(_node);                                                                                    \
            if (not server.applySchema(#LandmarkClass))                                                               \
            {                                                                                                         \
                WOLF_ERROR(server.getLog());                                                                          \
                return nullptr;                                                                                       \
            }                                                                                                         \
            std::shared_ptr<LandmarkClass> lmk(new LandmarkClass(server.getNode()));                                  \
            lmk->emplacePriors();                                                                                     \
            return lmk;                                                                                               \
        }                                                                                                             \
        std::shared_ptr<LandmarkClass> lmk(new LandmarkClass(_node));                                                 \
        lmk->emplacePriors();                                                                                         \
        return lmk;                                                                                                   \
    }
 
#define WOLF_LANDMARK_TEMPLATE_DIM_CREATE(LandmarkClass, Dim)                                                         \
    static LandmarkBasePtr create(const YAML::Node& _node, const std::vector<std::string>& _folders_schema = {})      \
    {                                                                                                                 \
        if (not _folders_schema.empty())                                                                              \
        {                                                                                                             \
            auto server = yaml_schema_cpp::YamlServer(_folders_schema);                                               \
            server.setYaml(_node);                                                                                    \
            if (not server.applySchema(#LandmarkClass + std::to_string(Dim) + "d"))                                   \
            {                                                                                                         \
                WOLF_ERROR(server.getLog());                                                                          \
                return nullptr;                                                                                       \
            }                                                                                                         \
            std::shared_ptr<LandmarkClass<Dim>> lmk(new LandmarkClass<Dim>(server.getNode()));                        \
            lmk->emplacePriors();                                                                                     \
            return lmk;                                                                                               \
        }                                                                                                             \
        std::shared_ptr<LandmarkClass<Dim>> lmk(new LandmarkClass<Dim>(_node));                                       \
        lmk->emplacePriors();                                                                                         \
        return lmk;                                                                                                   \
    }
 
// class LandmarkBase
class LandmarkBase : public NodeStateBlocks
{
    friend FactorBase;
 
  private:
    MapBaseWPtr         map_ptr_;
    static unsigned int landmark_id_count_;
 
  protected:
    unsigned int landmark_id_;    
    unsigned int track_id_;       
    int          external_id_;    
    int          external_type_;  
    TimeStamp    stamp_;          
 
    // Navigate wolf tree
    void setProblem(ProblemPtr _problem) override;
 
  public:
    LandmarkBase(const std::string&     _type,
                 const TypeComposite&   _state_types,
                 const VectorComposite& _state_vectors,
                 const PriorComposite&  _state_priors,
                 const int              _external_id   = -1,
                 const int              _external_type = -1);
 
    LandmarkBase(const std::string& _type, const TypeComposite& _state_types, const YAML::Node& _n);
 
    ~LandmarkBase() override = default;
    void remove(bool viral_remove_parent_without_children = false) override;
 
    YAML::Node toYaml() const override;
 
    // Properties
    unsigned int id() const override;
    void         setId(unsigned int _id);
 
    unsigned int trackId();                           // get track ID
    void         setTrackId(unsigned int _track_id);  // set track ID
 
    void setExternalId(const int& _external_id);
    int  getExternalId() const;
    void setExternalType(const int& _external_type);
    int  getExternalType() const;
 
    MapBaseConstPtr getMap() const;
    MapBasePtr      getMap();
 
    void link(MapBasePtr);
    template <typename classType, typename... T>
    static std::shared_ptr<classType> emplace(MapBasePtr _map_ptr, T&&... all);
 
    virtual void printHeader(int           depth,        //
                             bool          factored_by,  //
                             bool          metric,       //
                             bool          state_blocks,
                             std::ostream& stream,
                             std::string   _tabs = "") const;
 
    virtual void print(int           depth,        //
                       bool          factored_by,  //
                       bool          metric,       //
                       bool          state_blocks,
                       std::ostream& stream = std::cout,
                       std::string   _tabs  = "") const;
 
    virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
    virtual bool     check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
 
  private:
    void setMap(const MapBasePtr _map_ptr);
 
  public:
    LandmarkBasePtr      shared_from_this_landmark();
    LandmarkBaseConstPtr shared_from_this_landmark() const;
};
}  // namespace wolf
 
#include "core/map/map_base.h"
#include "core/factor/factor_base.h"
#include "core/state_block/state_block.h"
 
namespace wolf
{
template <typename classType, typename... T>
std::shared_ptr<classType> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&... all)
{
    std::shared_ptr<classType> lmk = std::make_shared<classType>(std::forward<T>(all)...);
    lmk->emplacePriors();
    lmk->link(_map_ptr);
    return lmk;
}
 
inline MapBaseConstPtr LandmarkBase::getMap() const
{
    return map_ptr_.lock();
}
 
inline MapBasePtr LandmarkBase::getMap()
{
    return map_ptr_.lock();
}
 
inline void LandmarkBase::setMap(const MapBasePtr _map_ptr)
{
    map_ptr_ = _map_ptr;
}
 
inline unsigned int LandmarkBase::id() const
{
    return landmark_id_;
}
 
inline void LandmarkBase::setId(unsigned int _id)
{
    landmark_id_ = _id;
    if (_id > landmark_id_count_) landmark_id_count_ = _id;
}
 
inline unsigned int LandmarkBase::trackId()
{
    return track_id_;
}
 
inline void LandmarkBase::setTrackId(unsigned int _track_id)
{
    track_id_ = _track_id;
}
 
inline void LandmarkBase::setExternalId(const int& _external_id)
{
    external_id_ = _external_id;
}
 
inline int LandmarkBase::getExternalId() const
{
    return external_id_;
}
 
inline void LandmarkBase::setExternalType(const int& _external_type)
{
    external_type_ = _external_type;
}
 
inline int LandmarkBase::getExternalType() const
{
    return external_type_;
}
 
inline LandmarkBasePtr LandmarkBase::shared_from_this_landmark()
{
    return std::static_pointer_cast<LandmarkBase>(shared_from_this());
}
 
inline LandmarkBaseConstPtr LandmarkBase::shared_from_this_landmark() const
{
    return std::static_pointer_cast<const LandmarkBase>(shared_from_this());
}
 
}  // namespace wolf